#include <stdio.h>
#include <math.h>
#include "nijie.h"
#include "oslib.h"
#define pi 3.1415926535898
#define A1 0.0415
#define L1 0.03125
#define L2 0.09225
#define L3 0.13013 // 单腿的各个杆长尺寸，单位为m

// void  simulate(double A[3])
// {
//     double B[4];
// 	//为存储舵机角度值的数组，B[0]为舵机1的角度，B[1] B[2]为舵机2角度的两个取值，B[3]为舵机3的角度
//     B[0] = -atan(A[0] / A[1]);
//     double C_lhl[3][3] = { { cos(B[0]),sin(B[0]),0 },{ -sin(B[0]), cos(B[0]), 0},{0, 0, 1} };//是求过逆之后的结果
//     double M[3] = { 0,0,0 };
//     int i, j;
//     for (i = 0; i < 3; i++)
//         for (j = 0; j < 3; j++)
//             M[i] += A[j] * C_lhl[i][j];//进行矩阵乘法 M数组相当于MATLAB程序中的A
//     M[1] = M[1] - L1;
//     M[2] = M[2] - A1;
//     B[3] = acos((M[1] * M[1] + M[2] * M[2] - L2 * L2 - L3 * L3) / (2 * L2 * L3));

//     double a = M[1] + L3 * cos(B[3]) + L2;
//     double b = 2 * L3 * sin(B[3]);
//     double c = M[1] - L3 * cos(B[3]) - L2;
//     double delta = b * b - 4 * a * c;
//     double t1, t2;
//     if (delta > 0)
//     {
//         t1 = (-b + sqrt(delta)) / (2 * a);
//         t2 = (-b - sqrt(delta)) / (2 * a);
//     }
//     B[1] = 2 * atan(t1);
//     B[2] = 2 * atan(t2);
//     for (i = 0; i < 4; i++)
//         uprintf("%lf ", B[i]);
// }

// int main()
// {
//     double A[3];
//     int i;
//   // for (i = 0; i < 3; i++)
//      // scanf_s("%f", &A[i]);//手动输入目的足端坐标值（x,y,z）
//     A[0] = -0.1456;
//     A[1] = 0.1736;
//     A[2] = -0.0215;
//     simulate(A);//A为存储位置的数组

//     return 0;
// }
// 	float Roll;        //unit: rad
// 	float Pitch;       //unit: rad
// 	float Heading;     //unit: rad
// 	float Qw;//w          //Quaternion
// 	float Qx;//x
// 	float Qy;//y
// 	float Qz;//z
// 	uint32_t Timestamp; //unit: us
// }AHRSData_Packet_t;

// extern char IMU_RxBuff[256];
// //extern uint16_t IMU_Rx_Cnt;
// extern AHRSData_Packet_t AHRSData_Packet;
// //extern uint16_t IMU_Rec_OK;
// //void MX_USART1_UART_Init(void);
// //void MX_USART2_UART_Init(void);

// /* USER CODE BEGIN Prototypes */
// //void imu_init(UART_HandleTypeDef *huart,char * defaultsign);
// void imu_exe();
// //void Uart_Printf(char *fmt,...);
// /* USER CODE END Prototypes */

// #ifdef __cplusplus
// }
// #endif

// #endif /* __imu_H__ */

// /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

#ifndef IMU_H
#define IMU_H

#define FRAME_HEADER 0X7B // Frame_header //֡ͷ
#define FRAME_TAIL 0X7D	  // Frame_tail   //֡β
#define SEND_DATA_SIZE 24
#define RECEIVE_DATA_SIZE 11
#define IMU_RS 64
#define AHRS_RS 56
#define INSGPS_RS 80

#define FRAME_HEAD 0xfc
#define FRAME_END 0xfd
#define TYPE_IMU 0x40
#define TYPE_AHRS 0x41
#define TYPE_INSGPS 0x42
#define TYPE_GROUND 0xf0
#define IMU_LEN 0x38	// 56+8  8组数据
#define AHRS_LEN 0x30	// 48+8  7组数据
#define INSGPS_LEN 0x42 // 72+8  10组数据
#define IMU_CAN 9
#define AHRS_CAN 8
#define INSGPS_CAN 11
// //RS-485
// #define RS485_RX_DE		PAout(11)	//485模式控制.0,接收;1,发送.
// #define RS485_RX_RE		PAout(12)	//485模式控制.0,接收;1,发送.

extern uint8_t ttl_receive;

typedef struct IMUData_Packet_t
{
	float gyroscope_x;			// unit: rad/s
	float gyroscope_y;			// unit: rad/s
	float gyroscope_z;			// unit: rad/s
	float accelerometer_x;		// m/s^2
	float accelerometer_y;		// m/s^2
	float accelerometer_z;		// m/s^2
	float magnetometer_x;		// mG
	float magnetometer_y;		// mG
	float magnetometer_z;		// mG
	float imu_temperature;		// C
	float Pressure;				// Pa
	float pressure_temperature; // C
	uint32_t Timestamp;			// us
} IMUData_Packet_t;

typedef struct AHRSData_Packet_t
{
	float RollSpeed;	// unit: rad/s
	float PitchSpeed;	// unit: rad/s
	float HeadingSpeed; // unit: rad/s
	float Roll;			// unit: rad
	float Pitch;		// unit: rad
	float Heading;		// unit: rad
	float Qw;			// w          //Quaternion
	float Qx;			// x
	float Qy;			// y
	float Qz;			// z
	uint32_t Timestamp; // unit: us
} AHRSData_Packet_t;

void imu_exe();
uint8_t TTL_Hex2Dec(void);

float DATA_Trans(uint8_t Data_1, uint8_t Data_2, uint8_t Data_3, uint8_t Data_4);
long long timestamp(uint8_t Data_1, uint8_t Data_2, uint8_t Data_3, uint8_t Data_4);
void PrintAHRSData(void);
void PrintIMUData(void);

#endif // IMU_H